import pandas as pd
import matplotlib.pyplot as plt
import numpy as np
from scipy.interpolate import make_interp_spline

# ================================================== 画图 ===============================================
def plot_by_matplot(path_csv):
    # 1. 读取CSV数据
    df = pd.read_csv(path_csv)  # 假设文件名为drone_trajectory.csv

    # 2. 绘制轨迹图
    plt.figure(figsize=(12, 8))
    plt.plot(df['longitude'][:80], df['latitude'][:80], 'o-', color='#1f77b4', markersize=5, linewidth=2)

    # 3. 添加地理参考线（可选）
    plt.axhline(y=37.77, color='gray', linestyle='--', alpha=0.5)  # 参考纬度线
    plt.axvline(x=-122.42, color='gray', linestyle='--', alpha=0.5)  # 参考经度线

    # 4. 设置图形属性
    plt.title('Drone Flight Trajectory', fontsize=14)
    plt.xlabel('Longitude', fontsize=12)
    plt.ylabel('Latitude', fontsize=12)
    plt.grid(True, linestyle=':', alpha=0.7)
    plt.grid(True, linestyle=':', alpha=0.7)

    # 5. 显示/保存图像
    # plt.savefig('flight_trajectory.png', dpi=300, bbox_inches='tight')
    plt.show()


# ================================================== 转换坐标 ===============================================
# 3. 定义转换函数（线性缩放至360x360）
def latlon_to_xy(lat, lon, lat_min, lat_max, lon_min, lon_max):
    # 经度方向（x轴）：从西到东（最小→最大）
    x = 360 * (lon - lon_min) / (lon_max - lon_min)
    # 纬度方向（y轴）：从南到北（最小→最大）
    y = 360 * (lat - lat_min) / (lat_max - lat_min)
    return x, y


def latlon_2_xy(path_csv):
    # 1. 读取CSV数据
    df = pd.read_csv(path_csv)
    df = df[:30]
    # 2. 计算经纬度范围
    lon_min, lon_max = df['longitude'].min(), df['longitude'].max()
    lat_min, lat_max = df['latitude'].min(), df['latitude'].max()
    # 4. 应用转换
    df[['x', 'y']] = df.apply(lambda row: latlon_to_xy(row['latitude'], row['longitude'],
                                                       lat_min, lat_max, lon_min, lon_max),
                              axis=1, result_type='expand')
    # 5. 绘制轨迹图
    plt.figure(figsize=(12, 12))
    plt.plot(df['x'], df['y'], 'o-', color='#1f77b4', markersize=5, linewidth=2)
    # 6. 添加边界参考线
    plt.axhline(y=0, color='gray', linestyle='--', alpha=0.5)
    plt.axhline(y=360, color='gray', linestyle='--', alpha=0.5)
    plt.axvline(x=0, color='gray', linestyle='--', alpha=0.5)
    plt.axvline(x=360, color='gray', linestyle='--', alpha=0.5)
    # 7. 设置图形属性
    plt.title('Drone Flight Path in 360x360 Grid', fontsize=14)
    plt.xlabel('Longitude Scaled (0-360)', fontsize=12)
    plt.ylabel('Latitude Scaled (0-360)', fontsize=12)
    plt.grid(True, linestyle=':', alpha=0.7)
    plt.grid(True, linestyle=':', alpha=0.7)
    # 8. 保存/显示
    # plt.savefig('flight_path_grid.png', dpi=300, bbox_inches='tight')
    plt.show()


# ================================================== 转换坐标+差值 ===============================================
# 3. 定义转换函数（线性缩放至360x360）
def latlon_to_xy_2(lat, lon, lat_min, lat_max, lon_min, lon_max):
    # 经度方向（x轴）：从西到东（最小→最大）
    x = 260 * abs(lon - lon_min) / abs(lon_max - lon_min) + 45
    # 纬度方向（y轴）：从南到北（最小→最大）
    y = 260 * abs(lat - lat_min) / abs(lat_max - lat_min) + 40
    return x, y


def xy_2_thph_save(x_smooth, y_smooth, path_csv_xy_thph):
    # 3. 坐标系转换 (笛卡尔 → 球坐标)
    r = np.sqrt(x_smooth ** 2 + y_smooth ** 2)
    # 处理数值误差导致r超出[-1,1]范围
    r_clipped = np.clip(r, -1.0, 1.0)
    th_smooth = np.arcsin(r_clipped)  # 极角 (弧度)
    ph_smooth = np.arctan2(y_smooth, x_smooth)  # 方位角 (弧度)
    # 转换为角度制（可选）
    th_deg = np.degrees(th_smooth)
    ph_deg = np.degrees(ph_smooth)
    # 4. 保存结果到CSV
    result_df = pd.DataFrame({
        'x_smooth': x_smooth,
        'y_smooth': y_smooth,
        'th_smooth': th_deg,  # 极角（度）
        'ph_smooth': ph_deg  # 方位角（度）
    })
    result_df.to_csv(path_csv_xy_thph, index=False)


def custom_polar(x, y):
    # 中心平移
    x_mid = x - 180
    y_mid = y - 180
    # 计算半径（防除零处理）
    r = np.sqrt(x_mid ** 2 + y_mid ** 2)
    r = np.where(r == 0, 1e-10, r)  # 当x=180且y=180时，设置极小值
    # 计算th（强制范围约束）
    th_ratio = np.clip(r / 180, -1.0, 1.0)  # 确保arcsin输入有效
    th_rad = np.arcsin(th_ratio)
    th_deg = np.degrees(th_rad)
    # 计算ph（带象限判断）
    ph_rad = np.arccos(x_mid / r)
    ph_deg = np.degrees(ph_rad)
    # 处理y_mid负向情况
    ph_deg = np.where(y_mid < 0, ph_deg + 180, ph_deg)
    # 角度范围修正（0-360度）
    th_deg = np.mod(th_deg, 360)
    ph_deg = np.mod(ph_deg, 360)
    return th_deg, ph_deg


def xy_2_thph_save_2(x_smooth, y_smooth, path_csv_xy_thph):
    # 3. 坐标系转换
    th_smooth, ph_smooth = custom_polar(x_smooth, y_smooth)
    # 4. 保存结果到CSV
    result_df = pd.DataFrame({
        'x_smooth': x_smooth,
        'y_smooth': y_smooth,
        'th_smooth': th_smooth,  # 离开原点的最大角度（0-90度）
        'ph_smooth': ph_smooth  # 方位角（0-360度）
    })
    result_df.to_csv(path_csv_xy_thph, index=False)


def plot_ori_smooth(df, df_smooth):
    plt.figure(figsize=(15, 10))
    # 原始轨迹（红色虚线）
    plt.plot(df['x'], df['y'], 'ro--', markersize=4, label='origin', alpha=0.6)
    # 平滑轨迹（蓝色实线）
    plt.plot(df_smooth['x'], df_smooth['y'], 'bo-', linewidth=2, label='smooth', alpha=0.9)
    # 边界参考线
    plt.axhline(y=0, color='gray', linestyle=':', alpha=0.5)
    plt.axhline(y=360, color='gray', linestyle=':', alpha=0.5)
    plt.axvline(x=0, color='gray', linestyle=':', alpha=0.5)
    plt.axvline(x=360, color='gray', linestyle=':', alpha=0.5)
    # 图形标注
    plt.title('Drone Flight Path in 360x360 Grid (with smooth)', fontsize=16)
    plt.xlabel('Longitude Scaled (0-360)', fontsize=12)
    plt.ylabel('Latitude Scaled (0-360)', fontsize=12)
    plt.legend()
    plt.grid(True, linestyle=':', alpha=0.7)
    # plt.savefig('smoothed_trajectory.png', dpi=300, bbox_inches='tight')
    plt.show()


def latlon_2_xy_interp(path_csv):
    # 1. 读取数据并转换到360x360坐标系
    df = pd.read_csv(path_csv)
    # df = df[:80]

    lon_min, lon_max = df['longitude'].min(), df['longitude'].max()
    lat_min, lat_max = df['latitude'].min(), df['latitude'].max()

    df[['x', 'y']] = df.apply(lambda row: latlon_to_xy_2(row['latitude'], row['longitude'],
                                                       lat_min, lat_max, lon_min, lon_max),
                              axis=1, result_type='expand')
    # 2. 生成平滑插值数据
    # 原始数据索引作为插值参数
    x_original = df['x'].values
    y_original = df['y'].values
    t = np.linspace(0, 1, len(x_original))  # 归一化参数
    # 创建三次样条插值函数
    spline_x = make_interp_spline(t, x_original, k=3)  # 三次样条（k=3）
    spline_y = make_interp_spline(t, y_original, k=3)
    # 生成更密集的插值点（增加5倍采样）
    t_new = np.linspace(0, 1, len(x_original)*5)
    x_smooth = spline_x(t_new)
    y_smooth = spline_y(t_new)
    # 3. 合并原始与平滑数据
    df_smooth = pd.DataFrame({'x': x_smooth, 'y': y_smooth})
    # 4. 可视化对比
    plot_ori_smooth(df, df_smooth)
    # 5. 转为(theta,phi)并保存为.csv
    xy_2_thph_save_2(x_smooth, y_smooth, "../files/archive/uav_dataset_xy_thph.csv")


# ============================================ 转换坐标+差值+缩放差值到360x360 =======================================
def constrained_spline(x, y, num=500):
    t = np.linspace(0, 1, len(x))  # 归一化参数

    # 创建三次样条（使用自然边界条件）
    spline_x = make_interp_spline(t, x, k=3, bc_type='natural')
    spline_y = make_interp_spline(t, y, k=3, bc_type='natural')

    t_new = np.linspace(0, 1, num)
    x_interp = spline_x(t_new)
    y_interp = spline_y(t_new)

    # 边界约束（替代extrapolate参数）
    x_scaled = np.clip(x_interp, 0, 360)
    y_scaled = np.clip(y_interp, 0, 360)

    return x_scaled, y_scaled


def latlon_2_xy_interp_scaled(path_csv):
    # 1. 读取数据并转换到360x360坐标系
    df = pd.read_csv(path_csv)
    df = df[:30]

    lon_min, lon_max = df['longitude'].min(), df['longitude'].max()
    lat_min, lat_max = df['latitude'].min(), df['latitude'].max()
    # 2. 三次样条插值（带边界约束）
    df[['x', 'y']] = df.apply(lambda row: latlon_to_xy(row['latitude'], row['longitude'],
                                                       lat_min, lat_max, lon_min, lon_max),
                              axis=1, result_type='expand')
    # 约束边界
    x_smooth, y_smooth = constrained_spline(df['x'], df['y'])
    # 3. 可视化对比
    plt.figure(figsize=(15, 10))
    # 原始轨迹（红色虚线）
    plt.plot(df['x'], df['y'], 'ro--', markersize=4, label='origin', alpha=0.6)
    # 平滑轨迹（蓝色实线）
    plt.plot(x_smooth, y_smooth, 'b-', linewidth=2, label='smooth', alpha=0.9)
    # 边界参考线
    plt.axhline(y=0, color='gray', linestyle=':', alpha=0.5)
    plt.axhline(y=360, color='gray', linestyle=':', alpha=0.5)
    plt.axvline(x=0, color='gray', linestyle=':', alpha=0.5)
    plt.axvline(x=360, color='gray', linestyle=':', alpha=0.5)

    # 图形标注
    plt.title('Drone Flight Path in 360x360 Grid (with smooth & scaled)', fontsize=16)
    plt.xlabel('Longitude Scaled (0-360)', fontsize=12)
    plt.ylabel('Latitude Scaled (0-360)', fontsize=12)
    plt.legend()
    plt.grid(True, linestyle=':', alpha=0.7)
    # plt.savefig('constrained_smooth.png', dpi=300, bbox_inches='tight')
    plt.show()



if __name__=="__main__":
    path_csv = "../files/archive/uav_navigation_dataset.csv"
    # plot_by_matplot(path_csv)
    # latlon_2_xy(path_csv)
    latlon_2_xy_interp(path_csv)
    # latlon_2_xy_interp_scaled(path_csv)